#include <time.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/types_c.h"
#include "opencv2/imgproc/imgproc.hpp"

using namespace cv;
using namespace std;

/**柱面投影函数
 *参数列表中imgIn为输入图像，f为焦距
 *返回值为柱面投影后的图像
*/
Mat cylinder(Mat imgIn, int f)
{
    int colNum, rowNum;
    colNum = 2 * f * atan(0.5 * imgIn.cols / f);                        //柱面图像宽
    rowNum = 0.5 * imgIn.rows * f / sqrt(pow(f, 2)) + 0.5 * imgIn.rows; //柱面图像高

    Mat imgOut = Mat::zeros(rowNum, colNum, CV_8UC1);
    Mat_<uchar> im1(imgIn);
    Mat_<uchar> im2(imgOut);

    //正向插值
    int x1(0), y1(0);
    for (int i = 0; i < imgIn.rows; i++)
        for (int j = 0; j < imgIn.cols; j++)
        {
            x1 = f * atan((j - 0.5 * imgIn.cols) / f) + f * atan(0.5 * imgIn.cols / f);
            y1 = f * (i - 0.5 * imgIn.rows) / sqrt(pow(j - 0.5 * imgIn.cols, 2) + pow(f, 2)) + 0.5 * imgIn.rows;
            if (x1 >= 0 && x1 < colNum && y1 >= 0 && y1 < rowNum)
            {
                im2(y1, x1) = im1(i, j);
            }
        }
    return imgOut;
}

/**求平移量
 *参数表为输入两幅图像（有一定重叠区域）
 *返回值为点类型，存储x,y方向的偏移量
*/
Point2i getOffset(Mat img, Mat img1)
{
    Mat templ(img1, Rect(0, 0.4 * img1.rows, 0.2 * img1.cols, 0.2 * img1.rows));
    Mat result(img.cols - templ.cols + 1, img.rows - templ.rows + 1, CV_8UC1); //result存放匹配位置信息
    matchTemplate(img, templ, result, CV_TM_CCORR_NORMED);
    normalize(result, result, 0, 1, NORM_MINMAX, -1, Mat());
    double minVal;
    double maxVal;
    Point minLoc;
    Point maxLoc;
    Point matchLoc;
    minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, Mat());
    matchLoc = maxLoc; //获得最佳匹配位置
    int dx = matchLoc.x;
    int dy = matchLoc.y - 0.4 * img1.rows; //右图像相对左图像的位移
    Point2i a(dx, dy);
    return a;
}

/*渐入渐出拼接
 *参数列表中，img1,img2为待拼接的两幅图像，a为偏移量
 *返回值为拼接后的图像
*/
Mat linearStitch(Mat img, Mat img1, Point2i a)
{
    int d = img.cols - a.x;       //过渡区宽度
    int ms = img.rows - abs(a.y); //拼接图行数
    int ns = img.cols + a.x;      //拼接图列数
    Mat stitch = Mat::zeros(ms, ns, CV_8UC1);
    //拼接
    Mat_<uchar> ims(stitch);
    Mat_<uchar> im(img);
    Mat_<uchar> im1(img1);

    if (a.y >= 0)
    {
        Mat roi1(stitch, Rect(0, 0, a.x, ms));
        img(Range(a.y, img.rows), Range(0, a.x)).copyTo(roi1);
        Mat roi2(stitch, Rect(img.cols, 0, a.x, ms));
        img1(Range(0, ms), Range(d, img1.cols)).copyTo(roi2);
        for (int i = 0; i < ms; i++)
            for (int j = a.x; j < img.cols; j++)
                ims(i, j) = uchar((img.cols - j) / float(d) * im(i + a.y, j) + (j - a.x) / float(d) * im1(i, j - a.x));
    }
    else
    {
        Mat roi1(stitch, Rect(0, 0, a.x, ms));
        img(Range(0, ms), Range(0, a.x)).copyTo(roi1);
        Mat roi2(stitch, Rect(img.cols, 0, a.x, ms));
        img1(Range(-a.y, img.rows), Range(d, img1.cols)).copyTo(roi2);
        for (int i = 0; i < ms; i++)
            for (int j = a.x; j < img.cols; j++)
                ims(i, j) = uchar((img.cols - j) / float(d) * im(i, j) + (j - a.x) / float(d) * im1(i + abs(a.y), j - a.x));
    }

    return stitch;
}

int main()
{
    Mat img = imread("a.png", 0);  //左图像
    Mat img1 = imread("b.png", 0); //右图像
    // imshow("源图像-左", img);
    // imshow("源图像-右", img1);
    //柱形投影
    // img = cylinder(img, 1000);
    // img1 = cylinder(img1, 1000);
    //匹配
    Point2i a = getOffset(img, img1);
    printf("offset=(%d,%d)", a.x, a.y);
    // //拼接
    // Mat stitch = linearStitch(img, img1, a);

    // imshow("柱面校正-左图像", img);
    // imshow("柱面校正-右图像", img1);
    // imshow("拼接结果", stitch);
    // imwrite("rectify.jpg", img);
    // imwrite("rectify1.jpg", img1);
    // imwrite("stitch.jpg", stitch);
    waitKey(0);
    return 0;
}